/*
*@功能：ESP32平衡小车
*@作者：刘泽文
*@时间：2020/3/19
*@思路：https://www.bilibili.com/video/BV1QK411J7Bg
*/
#include "mycar.h"
#include <MPU6050_tockn.h>
#include <WiFi.h>
#include <Ticker.h>
#include <hcsr04.h>
#include <U8g2lib.h>
#include <Arduino.h>
#include <Wire.h>
#include <math.h>
#include <esp32-hal-timer.h>

//实例化
HCSR04 hcsr04(TRIG_PIN, ECHO_PIN, 10, 3000);
MPU6050 mpu(Wire);
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

//PID计算定时器
Ticker pidticker;

void mpu6050_int(void); //mpu6050初始化
void OLED_int(void); //0.96OLED初始化
void gpio_int(void); //初始化用到的GPIO
void mpu6050_update(void); //更新mpu6050数据到结构体
void date_update(void);//更新所有需要的变量
void date_print(void);//打印参数
void turn(void);    //转向环PD
void velocity(void);//速度环PI
void balance(void); //平衡环PD
void pwm_limit(void);//PWM限幅
void set_pwm(void);//设置轮子PWM
void pidupdate(void);//计算PID，更新PWM
void KEYM_Handler(void);

void setup() {
  Serial.begin(115200);

  OLED_int();
  gpio_int();
  mpu6050_int();
  date_update();
  delay(1000);

  //pidticker.attach(0.005, pidupdate);
}

void loop() {
  LEFT_valA=0;
  LEFT_valB=0;

  Encoder_time.refresh_time = millis();
  while((millis()-Encoder_time.refresh_time)<Encoder_time.sample_time)//在采样时间内对编码器计数(四倍频)
  {
    if(LEFT_A==HIGH && LEFT_flagA==0){
      LEFT_valA++;
      LEFT_flagA=1;
      }
    if(LEFT_A==LOW && LEFT_flagA==1){
      LEFT_valA++;
      LEFT_flagA=0;
      }
    if(LEFT_B==HIGH && LEFT_flagB==0){
      LEFT_valB++;
      LEFT_flagB=1;
      }
    if(LEFT_B==LOW && LEFT_flagB==1){
      LEFT_valB++;
      LEFT_flagB=0;
      }
  }

  LEFT_n = (LEFT_A_PIN+LEFT_A_PIN)/(Encoder_time.sample_time/1000);//计算转速
  Serial.printf("%lf rad/s\r",LEFT_n);//输出转速数值
  //清零储存脉冲数的变量

}

/*
*@初始化mpu6050
*/
void mpu6050_int(void){
  Wire.begin();
  mpu.begin();
  mpu.calcGyroOffsets(true);
}

/*
*@初始化OLED屏
*/
void OLED_int(void){
  u8g2.begin();
  u8g2.enableUTF8Print();
  u8g2.setDisplayRotation(U8G2_R0);//设置显示器方向 U8G2_R0/U8G2_R2
  u8g2.firstPage();
  do{
    u8g2.setFont(u8g2_font_wqy14_t_gb2312a);
    u8g2.setCursor(16,16*3-8);
    u8g2.print("ESP32平衡小车");
  }while (u8g2.nextPage());
}

/*
*@初始化用到的所有IO口
*/
void gpio_int(void){
  //灯和蜂鸣器
  pinMode(LED_PIN,OUTPUT);
  LED_OFF;
  pinMode(BEEP_PIN,OUTPUT);
  BEEP_ON;
  delay(500);
  BEEP_OFF;
  //霍尔编码器
  pinMode(LEFT_A_PIN,INPUT_PULLUP);
  pinMode(LEFT_B_PIN,INPUT_PULLUP);
  pinMode(RIGHT_A_PIN,INPUT_PULLUP);
  pinMode(RIGHT_B_PIN,INPUT_PULLUP);
  //电源ADC
  adcAttachPin(POWER_ADC_PIN);//将引脚连接到ADC
  adcStart(POWER_ADC_PIN);//在连接的引脚总线上开始ADC转换
  analogReadResolution(12);//设置aliogRead返回值的分辨率2^12
  //电机PWM
  ledcSetup(channel1, freq1, resolution1); // 设置通道1
  ledcAttachPin(LEFT_PWM_1, channel1);  // 将通道0与引脚19连接
  ledcSetup(channel2, freq2, resolution2); // 设置通道2
  ledcAttachPin(LEFT_PWM_2, channel2);  // 将通道0与引脚19连接
  ledcSetup(channel3, freq3, resolution3); // 设置通道3
  ledcAttachPin(RIGHT_PWM_1, channel3);  // 将通道0与引脚19连接
  ledcSetup(channel4, freq4, resolution4); // 设置通道4
  ledcAttachPin(RIGHT_PWM_2, channel4);  // 将通道0与引脚19连接

  ledcWrite(channel1, 0);  // 输出PWM
  ledcWrite(channel2, 0);  // 输出PWM
  ledcWrite(channel3, 0);  // 输出PWM
  ledcWrite(channel4, 0);  // 输出PWM

  //按键引脚,全部内部上拉
  pinMode(KEYU_PIN,INPUT_PULLUP);
  pinMode(KEYM_PIN,INPUT_PULLUP);
  pinMode(KEYD_PIN,INPUT_PULLUP);
}

/*
*@更新mpu6050的角度值到本地结构体
*/
void mpu6050_update(void){
  //当前数据
//  MPU6050_NOW.temp = mpu.getTemp();
//  MPU6050_NOW.AccX = mpu.getAccX();
//  MPU6050_NOW.AccY = mpu.getAccY();
//  MPU6050_NOW.AccZ = mpu.getAccZ();
//  MPU6050_NOW.GyroX = mpu.getGyroX();
  MPU6050_NOW.GyroY = mpu.getGyroY();
//  MPU6050_NOW.GyroZ = mpu.getGyroZ();
//  MPU6050_NOW.AngleX = mpu.getAngleX();
  MPU6050_NOW.AngleY = mpu.getAngleY();
//  MPU6050_NOW.AngleZ = mpu.getAngleZ();
  //将上次的数据放入缓存区
  MPU6050_BUFF = MPU6050_NOW;
}

/*
*@更新所有需要的变量
*/
void date_update(void)
{
  //PID参数定义
  BALANCE.kp = 70;
  BALANCE.kd = 0;

  VELOCITY.kp = -3.5;
  VELOCITY.ki = VELOCITY.kp/200;

  delay(100);

  //机械中值
  mpu.update();
  MPU6050_NOW.beginX = mpu.getAngleX();
  MPU6050_NOW.beginY = mpu.getAngleY();
  MPU6050_NOW.beginZ = mpu.getAngleZ();

  MPU6050_NOW.AngleY_Max = 25;
  MPU6050_NOW.medianY = 5.3;

  //刷新时间定义
  MPU_time.sample_time = 10;//OLED刷新时间为100帧
  OLED_time.sample_time = 100;//OLED刷新时间为10帧
  Encoder_time.sample_time = 50;
}

/*
*@打印一些变量
*/
void date_print(void)
{
  Serial.print("BALANCE.PWM : ");
	Serial.print(BALANCE.PWM);
	Serial.print("\tY : ");
	Serial.print(MPU6050_NOW.AngleY);
	Serial.print("\tdy : ");
	Serial.println(MPU6050_NOW.AngleY-MPU6050_NOW.medianY);
}

/*
*@转向环
*/
void turn()
{
}

/*
*@速度环
*/
void velocity()
{
}

/*
*@平衡环
*/
void balance()
{
  float Bias;
  Bias = MPU6050_NOW.AngleY-MPU6050_NOW.medianY;   //===求出平衡的角度中值 和机械相关
  if(abs(Bias)>0.5)
    BALANCE.PWM = BALANCE.kp * Bias + MPU6050_NOW.GyroY * BALANCE.kd; //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
  if(abs(Bias)>MPU6050_NOW.AngleY_Max)
    BALANCE.PWM = 0;
}

/*
*@PWM限幅
*/
void pwm_limit()
{
  LEFT.PWM = LEFT.PWM > 1023 ? 1023 : LEFT.PWM;
  LEFT.PWM = LEFT.PWM < -1023 ? -1023 : LEFT.PWM;
  RIGHT.PWM = RIGHT.PWM > 1023 ? 1023 : RIGHT.PWM;
  RIGHT.PWM = RIGHT.PWM < -1023 ? -1023 : RIGHT.PWM;
}

/*
*@设置轮子PWM
*/
void set_pwm()
{
  if(LEFT.PWM<0)
  {
    ledcWrite(channel1, abs(LEFT.PWM));  // 输出PWM
    ledcWrite(channel2, 0);  // 输出PWM
  }
  else
  {
    ledcWrite(channel1, 0);  // 输出PWM
    ledcWrite(channel2, abs(LEFT.PWM));  // 输出PWM
  }
  if(RIGHT.PWM<0)
  {
    ledcWrite(channel3, 0);  // 输出PWM
    ledcWrite(channel4, abs(RIGHT.PWM));  // 输出PWM
  }
  else
  {
    ledcWrite(channel3, abs(RIGHT.PWM));  // 输出PWM
    ledcWrite(channel4, 0);  // 输出PWM
  }
}

/*
*@计算PID，更新PWM
*/
void pidupdate()
{
  turn();    //转向环PD
  velocity();//速度环PI
  balance(); //平衡环PD
  LEFT.PWM = BALANCE.PWM - VELOCITY.PWM + TURN.PWM;
  RIGHT.PWM = BALANCE.PWM - VELOCITY.PWM - TURN.PWM;
  pwm_limit();
  set_pwm();
}
